/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package io;

import java.util.ArrayList;
import java.util.List;
import motej.Extension;
import motej.Mote;
import motej.event.AccelerometerEvent;
import motej.event.AccelerometerListener;
import motej.event.CoreButtonEvent;
import motej.event.CoreButtonListener;
import motej.event.ExtensionEvent;
import motej.event.ExtensionListener;
import motej.event.IrCameraEvent;
import motej.event.IrCameraListener;
import motej.request.ReportModeRequest;
import motejx.extensions.motionplus.MotionPlus;
import motejx.extensions.motionplus.MotionPlusEvent;
import motejx.extensions.motionplus.MotionPlusListener;
import tal.drivechain.plugin.Channel;
import tal.drivechain.plugin.Module;
import tal.drivechain.variable.Variable;
import tal.drivechain.variable.types.ArrayType;
import tal.drivechain.variable.types.DataType;

/**
 *
 * @author Tal Eisenberg
 */
public class WiiMote extends Module implements CoreButtonListener, AccelerometerListener, IrCameraListener, ExtensionListener, MotionPlusListener {
    int wiiMoteIdx = -1;
    Mote mote = null;
    List<Extension> extensions = new ArrayList<Extension>();

    private Variable buttonA = new Variable("buttonA", DataType.BOOLEAN, false, true);
    private Variable buttonB = new Variable("buttonB", DataType.BOOLEAN, false, true);
    private Variable buttonOne = new Variable("button1", DataType.BOOLEAN, false, true);
    private Variable buttonTwo = new Variable("button2", DataType.BOOLEAN, false, true);
    private Variable buttonPlus = new Variable("buttonPlus", DataType.BOOLEAN, false, true);
    private Variable buttonMinus = new Variable("buttonMinus", DataType.BOOLEAN, false, true);
    private Variable buttonHome = new Variable("buttonHome", DataType.BOOLEAN, false, true);
    private Variable buttonDpadRight = new Variable("dpadRight", DataType.BOOLEAN, false, true);
    private Variable buttonDpadDown = new Variable("dpadDown", DataType.BOOLEAN, false, true);
    private Variable buttonDpadLeft = new Variable("dpadLeft", DataType.BOOLEAN, false, true);
    private Variable buttonDpadUp = new Variable("dpadUp", DataType.BOOLEAN, false, true);

    private List<Double> gyroArray = new ArrayList<Double>();
    private Variable gyro = new Variable("gyro", new ArrayType(DataType.FLOAT), gyroArray, true);
    private double gPitch = 0.0, gRoll = 0, gYaw = 0;

    private List<Double> accelArray = new ArrayList<Double>();
    private Variable accel = new Variable("accel", new ArrayType(DataType.FLOAT), accelArray, true);
    private double aX = 0.0, aY = 0.0, aZ = 0.0;

    private Boolean useAccel = false, useIR = false, useMotionPlus = false, useBalanceBoard = false, useClassic = false, useNunchuck = false;
    private static float g = 9.780f;


    public void setWiiMoteIdx(Integer wiiMoteIdx) { this.wiiMoteIdx = wiiMoteIdx; }
    public int getWiiMoteIdx() { return wiiMoteIdx; }

    @Override
    protected void msgReceived(Variable in, long timestamp, Channel channel) {
        if (channel==Channel.GLOBALMESSAGE && in.getName().equals("wiimoteFound")) {
            if ((Integer)in.getInternalValue()==wiiMoteIdx) {
                if (this.getParentChain().getSharedObjects().containsKey("WiiFinder.wiiMotes")) {
                    List wiiMotes = (List)this.getParentChain().getSharedObjects().get("WiiFinder.wiiMotes");
                    if (wiiMotes.size()>wiiMoteIdx) {
                        mote = (Mote)wiiMotes.get(wiiMoteIdx);

                        mote.addCoreButtonListener(this);
                        mote.addExtensionListener(this);
                        if (this.useAccel) mote.addAccelerometerListener(this);
                        if (this.useIR) mote.addIrCameraListener(this);
                        if (this.useMotionPlus) mote.activateMotionPlus();
                        findReportMode();
                        log("WiiMote connected: " + mote.getBluetoothAddress(), MSG.ANOUNCE);
                        leds(false,false,false,false);
                    }

                } else log("Received a wiimoteFound msg but shared wii list does not exists.", MSG.WARNING);
            }
        }
    }

    public void buttonPressed(CoreButtonEvent evt) {
        if ((Boolean)buttonA.getInternalValue()!=evt.isButtonAPressed()) {
            buttonA.setValue(evt.isButtonAPressed(), true);
            sendToOutputs(buttonA, -1);
        } if ((Boolean)buttonB.getInternalValue()!=evt.isButtonBPressed()) {
            buttonB.setValue(evt.isButtonBPressed(), true);
            sendToOutputs(buttonB, -1);
        } if ((Boolean)buttonHome.getInternalValue()!=evt.isButtonHomePressed()) {
            buttonHome.setValue(evt.isButtonHomePressed(), true);
            sendToOutputs(buttonHome, -1);
        } if ((Boolean)buttonMinus.getInternalValue()!=evt.isButtonMinusPressed()) {
            buttonMinus.setValue(evt.isButtonMinusPressed(), true);
            sendToOutputs(buttonMinus, -1);
        } if ((Boolean)buttonOne.getInternalValue()!=evt.isButtonOnePressed()) {
            buttonOne.setValue(evt.isButtonOnePressed(), true);
            sendToOutputs(buttonOne, -1);
        } if ((Boolean)buttonPlus.getInternalValue()!=evt.isButtonPlusPressed()) {
            buttonPlus.setValue(evt.isButtonPlusPressed(), true);
            sendToOutputs(buttonPlus, -1);
        } if ((Boolean)buttonTwo.getInternalValue()!=evt.isButtonTwoPressed()) {
            buttonTwo.setValue(evt.isButtonTwoPressed(), true);
            sendToOutputs(buttonTwo, -1);
        } if ((Boolean)buttonDpadDown.getInternalValue()!=evt.isDPadDownPressed()) {
            buttonDpadDown.setValue(evt.isDPadDownPressed(), true);
            sendToOutputs(buttonDpadDown, -1);
        } if ((Boolean)buttonDpadLeft.getInternalValue()!=evt.isDPadLeftPressed()) {
            buttonDpadLeft.setValue(evt.isDPadLeftPressed(), true);
            sendToOutputs(buttonDpadLeft, -1);
        } if ((Boolean)buttonDpadRight.getInternalValue()!=evt.isDPadRightPressed()) {
            buttonDpadRight.setValue(evt.isDPadRightPressed(), true);
            sendToOutputs(buttonDpadRight, -1);
        } if ((Boolean)buttonDpadUp.getInternalValue()!=evt.isDPadUpPressed()) {
            buttonDpadUp.setValue(evt.isDPadUpPressed(), true);
            sendToOutputs(buttonDpadUp, -1);
        }
    }

    public void accelerometerChanged(AccelerometerEvent evt) {
        boolean xchange = false, ychange = false, zchange = false;
        if (evt.getX()/255.0!=aX) {
            xchange = true;
            aX = evt.getX()/255.0;
        } if (evt.getY()/255.0!=aY) {
            ychange = true;
            aY = evt.getY()/255.0;
        } if (evt.getZ()/255.0!=aZ) {
            zchange = true;
            aZ = evt.getZ()/255.0f;
        }
        if (xchange || zchange || ychange) {
            accelArray.clear();
            accelArray.add(aX);
            accelArray.add(aY);
            accelArray.add(aZ);
            sendToOutputs(accel, -1);
        }
    }


    public void setUseAccel(Boolean b) {
        this.useAccel = b;
        if (mote != null) {
            if (useAccel) {
                mote.addAccelerometerListener(this);
                log("Using accelerometer", MSG.INFO);
            } else {
                mote.removeAccelerometerListener(this);
                log("Not using accelerometer", MSG.INFO);
            }
            findReportMode();
        }
    } public boolean getUseAccel() { return this.useAccel; }

    public void setUseIR(Boolean b) {
        this.useIR = b;
        if (mote != null) {
            findReportMode();
        }
    } public boolean getUseIR() { return this.useIR; }

    public void setUseMotionPlus(Boolean b) {
        this.useMotionPlus = b;
        if (mote != null) {
            findReportMode();
            if (b) mote.activateMotionPlus();
            else mote.deactivateMotionPlus();
        }
    } public boolean getUseMotionPlus() { return this.useMotionPlus; }

    public void setUseBalanceBoard(Boolean b) {
        this.useBalanceBoard = b;
        if (mote!=null) {
            findReportMode();
        }
    } public boolean getUseBalanceBoard() { return this.useBalanceBoard; }

    public void setUseNunchuck(Boolean b) {
        this.useNunchuck = b;
        if (mote!=null) {
            findReportMode();
        }
    } public boolean getUseNunchuck() { return this.useNunchuck; }

    public void setUseClassic(Boolean b) {
        this.useClassic = b;
        if (mote!=null) {
            findReportMode();
        }
    } public boolean getUseClassic() { return this.useClassic; }

    private void findReportMode() {
        boolean hasExtension = (useNunchuck || useClassic || useBalanceBoard || useMotionPlus);
        if (!hasExtension && !useIR && !useAccel) {
            mote.setReportMode(ReportModeRequest.DATA_REPORT_0x30, true);
            log("Setting data report mode to 0x30", MSG.ANOUNCE);
        } else if (!hasExtension && !useIR && useAccel) {
            mote.setReportMode(ReportModeRequest.DATA_REPORT_0x31, true);
            log("Setting data report mode to 0x31", MSG.ANOUNCE);
        } else if (hasExtension && !useIR && !useAccel) {
            mote.setReportMode(ReportModeRequest.DATA_REPORT_0x34, true);
            log("Setting data report mode to 0x34", MSG.ANOUNCE);
        } else if (!hasExtension && useIR) {
            mote.setReportMode(ReportModeRequest.DATA_REPORT_0x33, true);
            log("Setting data report mode to 0x33", MSG.ANOUNCE);
        } else if (hasExtension && !useIR && useAccel) {
            mote.setReportMode(ReportModeRequest.DATA_REPORT_0x35, true);
            log("Setting data report mode to 0x35", MSG.ANOUNCE);
        } else if (hasExtension && useIR && !useAccel) {
            mote.setReportMode(ReportModeRequest.DATA_REPORT_0x36, true);
            log("Setting data report mode to 0x36", MSG.ANOUNCE);
        } else if (hasExtension && useIR && useAccel) {
            mote.setReportMode(ReportModeRequest.DATA_REPORT_0x37, true);
            log("Setting data report mode to 0x37", MSG.ANOUNCE);
        } else
            log("LOGIC ERROR: unknown case hasExtension=" + hasExtension + " useIR=" + useIR + " useAccel=" + useAccel, MSG.ERROR);
    }

    public void irImageChanged(IrCameraEvent evt) {
        log("irImageChanged: " + evt.getMode().name() + " x=" + evt.getIrPoint(0).x + " y=" + evt.getIrPoint(0).y + " intensity=" + evt.getIrPoint(0).intensity + " size=" + evt.getIrPoint(0).size, MSG.ANOUNCE);
    }

    public void extensionConnected(ExtensionEvent evt) {
        if (evt.getExtension() instanceof MotionPlus) {
            MotionPlus p = (MotionPlus)evt.getExtension();
            p.initialize();
            p.newCalibration();
            p.addMotionPlusEventListener(this);
            log("MotionPlus extension connected.", MSG.ANOUNCE);
        }

        extensions.add(evt.getExtension());
    }

    public void extensionDisconnected(ExtensionEvent evt) {
        log("Wii remote extension disconnected: " + evt.getExtension().getClass(), MSG.ANOUNCE);
        extensions.remove(evt.getExtension());
    }

    public void leds(boolean a, boolean b, boolean c, boolean d) {
        if (mote!=null)
            mote.setPlayerLeds(new boolean[] {a,b,c,d});
        else log("Can't set wii remote leds while remote is not connected.", MSG.ERROR);
    }

    public void speedChanged(MotionPlusEvent evt) {
        boolean pchange = false, rchange = false, ychange = false;
        if (evt.getPitchDownSpeed()!=gPitch) {
            pchange = true;
            gPitch = evt.getPitchDownSpeed();
        } if (evt.getRollLeftSpeed()!=gRoll) {
            rchange = true;
            gRoll = evt.getRollLeftSpeed();
        } if (evt.getYawLeftSpeed()!=gYaw) {
            ychange = true;
            gYaw = evt.getYawLeftSpeed();
        }
        if (pchange || rchange || ychange) {
            gyroArray.clear();
            gyroArray.add(gPitch);
            gyroArray.add(gRoll);
            gyroArray.add(gYaw);
            sendToOutputs(gyro, -1);
        }

    }
}

